// this is the header file that tells the compiler what pins and ports, etc.
 // are available on this chip.
 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <stdio.h>
 #include <stdint.h>
 
 // define what pins the LEDs are connected to.

 #define Warn PD6
 #define Red_L PD7
 #define Green_L PB2
 #define Red_R PB1
 #define Green_R PB0


 #define FOSC 8000000
 #define BAUD 9600
 #define MYUBRR (((FOSC/16)/BAUD)-1)

// Some macros that make the code more readable
 #define output_low(port,pin) port &= (uint8_t)~(1<<pin)
 #define output_high(port,pin) port |= (uint8_t)(1<<pin)
 #define set_input(portdir,pin) portdir &= (uint8_t)~(1<<pin)
 #define set_output(portdir,pin) portdir |=(uint8_t)(1<<pin)

 #define set(p,r) ((p) |= (bit(r)))
 #define clear(p,r) ((p) &= ~(bit(r)))
 #define bit(x) (0x01 << (x))
 
 #define startTimer1 set(TCCR1B,CS12)//256 prescale
 #define stopTimer1 clear(TCCR1B,CS12)
 
 void ioinit(void);      // initializes IO
 static int uart_putchar(char c, FILE *stream);

 uint8_t uart_getchar(void);

 static FILE mystdout = FDEV_SETUP_STREAM(uart_putchar, NULL, _FDEV_SETUP_WRITE);

 // this is just a program that 'kills time' in a calibrated method
 void delay_ms(uint8_t ms) {
   uint16_t delay_count = F_CPU / 17500;
   volatile uint16_t i;
 
   while (ms != 0) {
     for (i=0; i != delay_count; i++);
     ms--;
   }
 }

 int main(void) {
   // initialize the direction of PORTD #6 to be an output
	uint8_t key_press;
	
    ioinit(); //Setup IO pins and defaults  

	printf("Control software Demo Started\n");

	set(TCCR1B , WGM12); // enable CTC on timer 1
	set(TIMSK1 , OCIE1A);//enable timer intrupt
	OCR1A=0x617b;

	sei();

  while (1) {

  		key_press=uart_getchar();
		output_low(PORTD, Warn);
		stopTimer1;
		TCNT1=0;



	switch(key_press){
		
	case 'w':
		output_low(PORTD, Red_L);
		output_low(PORTB, Red_R);
		output_high(PORTB, Green_L);
		output_high(PORTB, Green_R);
		printf("Forward\n");
		startTimer1;
		break;
	
	case 's':
		output_low(PORTB, Green_L);
		output_low(PORTB, Green_R);
		output_high(PORTD, Red_L);
		output_high(PORTB, Red_R);
		printf("Reverse\n");
		startTimer1;
		break;
	
	case 'd':
		output_low(PORTB, Green_R);
		output_low(PORTD, Red_L);		
		output_high(PORTB, Green_L);	
		output_high(PORTB, Red_R);
		printf("Right\n");
		startTimer1;
		break; 
	
	case 'a':
		output_low(PORTB, Green_L);	
		output_low(PORTB, Red_R);		
		output_high(PORTB, Green_R);
		output_high(PORTD, Red_L);
		printf("Left\n");
		startTimer1;
		break;

	default:
	output_low(PORTB, Green_L);
	output_low(PORTB, Green_R);
	output_low(PORTD, Red_L);
	output_low(PORTB, Red_R);
	printf("Stop!!\n");

	}	

     // turn on the LED for 200ms
	printf("pressed: %c \n",key_press);

   }
}


void ioinit (void){
    set_output(DDRB, Green_R );
   	set_output(DDRB, Red_R);
   	set_output(DDRB, Green_L);
	set_output(DDRD, Red_L); 
	set_output(DDRD, Warn);

    //USART Baud rate: 9600
    UBRR0H = (MYUBRR>>8);
    UBRR0L = MYUBRR;
    UCSR0B = (1<<RXEN0)|(1<<TXEN0);
    
    stdout = &mystdout; //Required for printf init
}

static int uart_putchar(char c, FILE *stream){
    if (c == '\n') uart_putchar('\r', stream);
  
    loop_until_bit_is_set(UCSR0A, UDRE0);
    UDR0 = c;
    
    return 0;
}

uint8_t uart_getchar(void){
    while( !(UCSR0A & (1<<RXC0)) );
    return(UDR0);
}

ISR(TIMER1_COMPA_vect){
	stopTimer1;
	output_high(PORTD,Warn);
	output_low(PORTB, Green_R);
	output_low(PORTB, Green_L);
	output_low(PORTD, Red_L);
	output_low(PORTB, Red_R);
}
